#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/common.h>
// #include <pcl/io/ply_io.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
// #include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
// #include <pcl/sample_consensus/method_types.h>
// #include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
// #include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/search/kdtree.h>
// #include <pcl/gpu/segmentation/gpu_extract_clusters.h>
#include <pcl/io/io.h>
#include <typeinfo>
#include <math.h>
#include <pcl/console/time.h>
using namespace std;

double angle(vector<float> normal,vector<float> normalxoy)
{
    double up = 0;
    for(int i = 0; i < 3; i++)
    {
        up += normal[i]*normalxoy[i];
    }
    double length1 = sqrt(pow(normal[0],2)+pow(normal[1],2)+pow(normal[2],2));
    double length2 = sqrt(pow(normalxoy[0],2)+pow(normalxoy[1],2)+pow(normalxoy[2],2));
    
    return acos(up/(length1*length2));
}

int main(int argc,char** argv) {

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr ops_cloud(new pcl::PointCloud<pcl::PointXYZ>);    
    pcl::ExtractIndices<pcl::PointXYZ> extract;               // 创建提取对象专门用于提取indices
    pcl::console::TicToc tc;
    tc.tic();
    if(argv[1] == NULL)
    {
        pcl::io::loadPCDFile("../pcds/1.pcd",*cloud);
    }
    else
    {
        pcl::io::loadPCDFile(argv[1],*cloud);
    }
    if(cloud->points.size() == 0)
    {
        cerr<<"打开失败"<<endl;
        return 0;
    }

    // xoy平面法向量
    vector<float> xoy;
    xoy.push_back(0);
    xoy.push_back(0);
    xoy.push_back(1);
/*===========================================================================================*/

    // 去除NaN的点
    std::vector<int> mapping;
    pcl::removeNaNFromPointCloud(*cloud, *ops_cloud, mapping);
    cout<<ops_cloud->points.size()<<endl;

/*===========================================================================================*/
    // 设置体素栅格下采样对象
    pcl::PointCloud<pcl::PointXYZ>::Ptr vg_after_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::VoxelGrid<pcl::PointXYZ> vg;
    vg.setInputCloud(ops_cloud);
    vg.setLeafSize(0.01f,0.01f,0.02f);
    vg.filter(*ops_cloud);    // 下采样
    cout<<"栅格滤波之后的点个数:"<<ops_cloud->points.size()<<endl;
/*===========================================================================================*/

    // 设置滤波器对象
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(ops_cloud);   //设置输入的点云
    // pass.setFilterFieldName("x");           // 红色轴
    // pass.setFilterLimits(-200, 200);
    // pass.filter(*ops_cloud);
    pass.setFilterFieldName("y");              // 绿色轴
    pass.setFilterLimits(-1.8, -0.03);
    pass.filter(*ops_cloud);
    // pass.setFilterFieldName("z");           // 黑色轴
    // pass.setFilterLimits(0, 500);
    // pass.filter(*ops_cloud);
    
    // 剩下的都为x,y,z坐标满足一定条件的
    cout<<"坐标筛选之后点个数为"<<ops_cloud->points.size()<<endl;
    
/*===========================================================================================*/

    // 搜索点云的树形搜索方法
    // pcl::gpu::Octree::Ptr tree (new pcl::gpu::Octree);
    // tree->setCloud(*ops_cloud);
    // pcl::gpu::EuclideanClusterExtraction ec;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud (ops_cloud);

    
    // 保存聚类结果，一个indices表示一片
    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
    ec.setClusterTolerance(0.015);                // 设置最大近邻距离0.15m
    ec.setMinClusterSize(20);
    ec.setMaxClusterSize(1000);
    ec.setInputCloud(ops_cloud);
    ec.setSearchMethod(tree);                    // 搜索方式kd树，然而其实想用gpu方法
    ec.extract(cluster_indices);
    // cout<<cluster_indices.size()<<endl;

/*===========================================================================================*/

    pcl::PointIndices::Ptr temp (new pcl::PointIndices);  // 用于从vector中取出某个Indices并用智能指针来操作
    for(int i = 0; i < cluster_indices.size(); i++)
    {
        // cout<<cluster_indices[i].indices.size()<<endl;
        temp->indices = cluster_indices[i].indices;
        cout<<"==================================="<<endl;

        // 专门弄一个用来筛选，取出某个聚类结果
        pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud (new pcl::PointCloud<pcl::PointXYZ>);
        extract.setNegative(false); // 筛选不属于平面模型的点，如果为true
        extract.setInputCloud(ops_cloud);
        extract.setIndices(temp);
        extract.filter(*temp_cloud);


        // 创建一个模型参数对象，用于记录结果
        pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients());
        pcl::PointIndices::Ptr inliers (new pcl::PointIndices());
        pcl::SACSegmentation<pcl::PointXYZ> seg;

        seg.setInputCloud(temp_cloud);
        seg.setOptimizeCoefficients(true);
        seg.setModelType(pcl::SACMODEL_PLANE);
        seg.setMethodType(pcl::SAC_RANSAC);
        seg.setMaxIterations(10);
        seg.setDistanceThreshold(0.005);
        seg.segment(*inliers,*coefficients);

        extract.setNegative(false); // 筛选不属于平面模型的点，如果为true
        extract.setInputCloud(temp_cloud);
        extract.setIndices(inliers);
        extract.filter(*temp_cloud);

        // 剩下的都为平面模型内的点
        cout<<"平面模型内点个数为:"<<temp_cloud->points.size()<<endl;
        // 筛选出满足条件的indices
        if(temp_cloud->points.size() > 250 || temp_cloud->points.size() < 400 || abs(angle(coefficients->values,xoy)/(3.14)*180) < 30)
        {
            break;
        } 
        cout<<angle(coefficients->values,xoy)/(3.14)*180<<endl;
        // cout<<"==================================="<<endl;
    }

    // 获得弹药箱子indices，提取点云
    extract.setNegative(false); // 筛选不属于平面模型的点，如果为true
    extract.setInputCloud(ops_cloud);
    extract.setIndices(temp);
    extract.filter(*ops_cloud);

    pcl::PointXYZ min;
    pcl::PointXYZ max;
    pcl::PointXYZ center;
    pcl::getMinMax3D(*ops_cloud,min,max);
    // cout<<min<<" "<<max<<endl;
    center.x = (min.x + max.x)/2;
    center.y = (min.y + max.y)/2;
    center.z = (min.z + max.z)/2;
    cout<<"得到中点坐标:["<<center.x<<","<<center.y<<","<<center.z<<"]"<<endl;
    cout<<"使用时间:"<<tc.toc()<<"ms"<<endl;
    ops_cloud->points.push_back(center);
    pcl::visualization::CloudViewer viewer("Cloud Viewer");
    viewer.showCloud(ops_cloud);
    char b = getchar();  // 产生中断，从而保持视窗打开


    // PCLVisualizer对象出现未知错误
    // boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    // pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> fildColor(cloud, "z"); // 按照z字段进行渲染
    // viewer->addPointCloud<pcl::PointXYZ>(cloud, fildColor, "sample cloud");
    // viewer->removePointCloud("sample cloud");
    // viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); // 设置点云大小

    // while (!viewer->wasStopped())
    // {
    //     viewer->spinOnce(100);
    //     boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    // }

    return 0;
}